// 给定位姿轨迹和各帧的点云，拼接成一个大的点云地图。使用ros的回调函数
// 假设有两个tpoic发布一个位姿，一个发布局部下的点云数据
// 需要放到一个ros环境中才能编译运行，实际测试可行
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <queue>
#include <mutex>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>


std::mutex data_x;
std::queue<nav_msgs::Odometry::Ptr> q_odom;
std::queue<sensor_msgs::PointCloud2::Ptr> q_cloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>());

ros::Publisher pub_cloud;

void pose_cb(const nav_msgs::Odometry::ConstPtr& pose){
    // ROS_INFO_STREAM("get a pose");
    nav_msgs::Odometry::Ptr msg(new nav_msgs::Odometry(*pose));
    data_x.lock();
    q_odom.push(msg);
    data_x.unlock();
}

void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud){
    // ROS_INFO_STREAM("get a cloud");
    sensor_msgs::PointCloud2::Ptr msg(new sensor_msgs::PointCloud2(*cloud));
    data_x.lock();
    q_cloud.push(msg);
    data_x.unlock();
}


// 下面开始合成一个完整的点云地图
void mage_cloud_point(){
    double time = 0.0;
    data_x.lock();
    if(q_odom.size() < 1 || q_cloud.size() < 1){    // 数据不完整
        // ROS_INFO_STREAM("q_odom.size() < 1 || q_cloud.size() < 1");
        data_x.unlock();
        return ;
    }
    // 这里进行数据对齐一下，理论上这两个话题的时间戳应该是一样的
    while (q_odom.front()->header.stamp.toSec() > q_cloud.front()->header.stamp.toSec() + 0.005)
    {
        ROS_INFO_STREAM("q_cloud.pop();");
        q_cloud.pop();
        if (q_cloud.size() < 1)
        {
            data_x.unlock();
            return ;
        }
    }

    while (q_odom.front()->header.stamp.toSec() < q_cloud.front()->header.stamp.toSec() - 0.005)
    {
        ROS_INFO_STREAM("q_odom.pop();");
        q_odom.pop();
        if (q_odom.size() < 1)
        {
            data_x.unlock();
            return ;
        }
    }
    time = q_odom.front()->header.stamp.toSec();

    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    Eigen::Quaterniond q;
    q.x() = q_odom.front()->pose.pose.orientation.x;
    q.y() = q_odom.front()->pose.pose.orientation.y;
    q.z() = q_odom.front()->pose.pose.orientation.z;
    q.w() = q_odom.front()->pose.pose.orientation.w;

    Eigen::Vector3d p;
    p.x() = q_odom.front()->pose.pose.position.x;
    p.y() = q_odom.front()->pose.pose.position.y;
    p.z() = q_odom.front()->pose.pose.position.z;

    pose.block<3,3>(0,0) = q.matrix();
    pose.block<3,1>(0,3) = p;

    pcl::PointCloud<pcl::PointXYZI>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::fromROSMsg(*q_cloud.front(), *temp_cloud);

    pcl::PointCloud<pcl::PointXYZI>::Ptr temp_cloud_after(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::transformPointCloud(*temp_cloud, *temp_cloud_after, pose);

    *cloud += *temp_cloud_after;    

    q_odom.pop();
    q_cloud.pop();
    data_x.unlock();

    sensor_msgs::PointCloud2 cloud_all;
    pcl::toROSMsg(*cloud,cloud_all);
    cloud_all.header.frame_id = "camera_init";
    cloud_all.header.stamp = ros::Time().fromSec(time);
    pub_cloud.publish(cloud_all);
}


int main(int argc, char* argv[]){
    ros::init(argc, argv, "test");
    ros::NodeHandle nh;

    ros::Subscriber sub_pose = nh.subscribe("/odometry_gt",1000,pose_cb);
    ros::Subscriber sub_cloud = nh.subscribe("/velodyne_points",1000,cloud_cb);
    pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("test/could", 1000);

    ros::Rate rate(50);
    while (ros::ok())
    {
        ros::spinOnce();
        mage_cloud_point();
        // ROS_INFO_STREAM("11111111111");
        rate.sleep();
    }

    std::cout << "ok" << std::endl;
    return 0;
}




